/**
 * @file event_manager.cpp
 * @brief 
 * @author haibo.yang
 * @version 1.0
 * @date 2023-08-01
 * 
 * @copyright Copyright (c) 2023  lgim
 * 
 */
#include "state_monitor/event_manager.h"
#include "rclcpp/generic_publisher.hpp"
#include <rmw/rmw.h>

namespace monitor {

std::mutex eventMutex;

EventManager::EventManager():name_("EventManager")
{
}

EventManager::~EventManager() { ShutDown(); }

void EventManager::ShutDown()
{
  event_thread_.join();
}

void EventManager::Init(const double interval) 
{ 
  interval_ = interval;
  event_map_.clear();
  timeout_events_.clear();
  current_event_.first.clear();
  current_event_.second.type = agv_msgs::msg::AGVEvent::INFO;
  current_event_.second.action = agv_msgs::msg::AGVEvent::NONE;
  current_event_.second.ackreq = 0;
  current_event_.second.ecode = agv_msgs::msg::AGVEvent::NONE_EVENT;
  current_event_.second.eventtime = 0.0f;
  current_event_.second.vellimit = 0.0f;
  current_event_.second.description.clear();
  current_event_.second.description_cn.clear();

  // Setup subscribe agv event
  agv_event_sub_ = Monitor::GetNode()->create_subscription<agv_msgs::msg::AGVEvent>(
      "/agvevent", 10, 
      std::bind(&EventManager::EventCallBack, this, std::placeholders::_1, std::placeholders::_2));

  // Setup action realtime publisher + action message constant fields
  status_pub_ = Monitor::GetNode()->create_publisher<agv_msgs::msg::AGVEventStatus> ("/eventstatus", 10);

  // start thread
  event_thread_ = boost::thread(boost::bind(&EventManager::run, this));
}

void EventManager::run()
{
  MONITOR_INFO("thread id:" << boost::this_thread::get_id() << " spid:" << syscall(SYS_gettid));
  int seq = 0, seq1 = 0;
  while (rclcpp::ok()) {
    if (!event_map_.empty()) {

      rclcpp::Time current_time = Monitor::GetTimes();
      agv_msgs::msg::AGVEventStatus status;
      
      // status.header.seq = seq++;
      status.header.stamp = current_time;
      status.header.frame_id = "EventManager";
      auto ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
                std::chrono::duration<double>(AGVEVENTTIMEROUT));

      // Iterate over the map
      for (auto it = event_map_.begin(); it != event_map_.end(); ++it) {
        AgvEventVector &events = it->second;
        bool is_current_event = it->first == current_event_.first;
        
        // bool current_event_empty = current_event_.first.empty();
        // Find time out event and remove elements 
        // events.erase(std::find_if(events.begin(), events.end(), [ current_time ] (const AgvEvent& event) {
        //     if ((current_time - event.first).toSec() > AGVEVENTTIMEROUT) { 
        //       // MONITOR_WARN("Delete: " << "current time:" << current_time << " event time:" << event.first << "\n event:" << event.second );
        //       return true;  
        //     } else { return false; }
        // }), events.end());

        if (!events.empty()) {
          bool event_time_out = true;
          for (auto iter = events.begin(); iter != events.end();) {
            // If the event times out, it is deleted, otherwise it is sent
            if ((current_time - iter->first) < rclcpp::Duration(ns)) {
              event_time_out = false;
              // Determine whether event ecode NONE
              if (iter->second.ecode != agv_msgs::msg::AGVEvent::NONE_EVENT) {
                if (current_event_.second.action < iter->second.action || 
                  current_event_.second.type < iter->second.type) {
                  current_event_.first = it->first;
                  current_event_.second = iter->second;
                  MONITOR_WARN("Add current event node (" << current_event_.first << "):" << current_event_.second);
                }
                status.events.push_back(iter->second);
              }
              ++iter;
            } else {
              // Clear the current event status when the event disappears
              if (is_current_event && current_event_.second.ecode == iter->second.ecode) {
                current_event_.first.clear();
                current_event_.second.type = agv_msgs::msg::AGVEvent::INFO;
                current_event_.second.action = agv_msgs::msg::AGVEvent::NONE;
                current_event_.second.ackreq = 0;
                current_event_.second.ecode = agv_msgs::msg::AGVEvent::NONE_EVENT;
                current_event_.second.eventtime = 0.0f;
                current_event_.second.vellimit = 0.0f;
                current_event_.second.description.clear();
                current_event_.second.description_cn.clear();
                MONITOR_WARN("Current Event timeout and Delete: " << "current time:" 
                    << current_time.seconds() << " event time:" << 
                    iter->first.seconds() << "\n event:" << iter->second);
              }

              MONITOR_WARN("Event timeout Delete: " << "current time:" << current_time.seconds()
                    << " event time:" << iter->first.seconds() << "\n event:" << iter->second);
              iter = events.erase(iter);
            }
          }

          if (event_time_out && 
              it->first != "saftymonitor" && 
              it->first != "nodeMonitor" && 
              it->first != "SystemMonitor" && 
              it->first != "modeMonitor") {

            agv_msgs::msg::AGVEvent event;
            event.header.stamp = current_time;
            event.header.frame_id = "EventManager";
            // event.header.seq = seq1++;
            event.type = event.ALARM;
            event.action = event.STOP;
            event.ackreq = true;
            event.eventtime = current_time.seconds();
            event.ecode = event.EVENT_TIME_OUT;
            event.description = it->first + " event time out";
            // status.events.push_back(event);
            timeout_events_.push_back(std::make_pair(it->first, event));

            if (current_event_.second.action < event.action) {
              current_event_.first = it->first;
              current_event_.second = event;
              MONITOR_WARN("Add current event node (" << current_event_.first << "):" << current_event_.second);
            }

            MONITOR_ERROR("Diagnostic event timeout id:" << it->first);
          } else {
           
            // Clear the current event status when the event disappears
            if (is_current_event && current_event_.second.ecode == agv_msgs::msg::AGVEvent::EVENT_TIME_OUT) {
              current_event_.first.clear();
              current_event_.second.type = agv_msgs::msg::AGVEvent::INFO;
              current_event_.second.action = agv_msgs::msg::AGVEvent::NONE;
              current_event_.second.ackreq = 0;
              current_event_.second.ecode = agv_msgs::msg::AGVEvent::NONE_EVENT;
              current_event_.second.eventtime = 0.0f;
              current_event_.second.vellimit = 0.0f;
              current_event_.second.description.clear();
              current_event_.second.description_cn.clear();
              MONITOR_WARN("Current Event recover and delete: " << "current time:" << current_time.seconds()
                   << " event time:" << current_event_.first << "\n event:" << current_event_.second);
            }

            std::string name = it->first;
            // find timeout event and delet
            if (timeout_events_.size() > 0) {
                timeout_events_.erase(std::remove_if(timeout_events_.begin(), timeout_events_.end(), [name](const std::pair<std::string, agv_msgs::msg::AGVEvent>& event) {
                    if (event.first == name) {
                        MONITOR_INFO("Timeout event delete:" << event.second);
                        return true; 
                    } else
                        return false; 
                    }
                ), timeout_events_.end());
            }
          }
        }
      }

      // find time out events
      for (auto it = timeout_events_.begin(); it != timeout_events_.end(); ++it) {
        if (current_event_.second.action < it->second.action) {
          current_event_.first = it->first;
          current_event_.second = it->second;
          MONITOR_WARN("Add current event node (" << current_event_.first << "):" << current_event_.second);
        }
        status.events.push_back(it->second);
      }

      status.node = current_event_.first;
      status.type = current_event_.second.type;
      status.action = current_event_.second.action;
      status.ackreq = current_event_.second.ackreq;
      status.eventtime = current_event_.second.eventtime;
      status.vellimit = current_event_.second.vellimit;
      status.ecode = current_event_.second.ecode;
      status.description = current_event_.second.description;

      status_pub_->publish(status);
      // MONITOR_DEBUG("Event status publish:" << status);
    }
    
    // Sleep for 1 milliseconds
    boost::this_thread::sleep_for(boost::chrono::milliseconds(int(interval_* 1000)));
  }
  
}

void EventManager::EventCallBack(const agv_msgs::msg::AGVEvent::SharedPtr event, const rclcpp::MessageInfo& info)
{
  // Get the name of the node that published the topic
  const auto& gid = gid_to_hex_string(info.get_rmw_message_info().publisher_gid);
  auto receipt_time = info.get_rmw_message_info().received_timestamp;
  auto publishers = Monitor::GetNode()->get_publishers_info_by_topic("/agvevent");
  bool find_node_name = false;
  for (const auto & publisher : publishers) {
    const auto& pub_gid = gid_to_hex_string(publisher.endpoint_gid()); 
    MONITOR_INFO("gid:" << gid << ",pub_gid:" << pub_gid);
  
    if (gid == pub_gid) {
      // MONITOR_INFO("Received from node:" << publisher.node_name());
      std::string node_name = publisher.node_name();
      if (node_name != "_NODE_NAME_UNKNOWN_") {
        find_node_name = true;
        MONITOR_INFO("EventCallBack: node:" << node_name 
                     << " time: " << receipt_time / 1e9 
                     << " event: " << *event);
        
        if (current_event_.first == gid) {
          current_event_.first = node_name;  // 替换key为name
          MONITOR_DEBUG("Renamed current_event key: " << gid << " -> " << node_name);
        }

        for (auto& event_pair : timeout_events_) {
          if (event_pair.first == gid) {
            event_pair.first = node_name;  // 替换key为name
            MONITOR_DEBUG("Renamed timeout_event key: " << gid << " -> " << node_name);
          }
        }
        EventManager::EscalateEvents(node_name, event, gid);
      }
      break;
    } 
  }
  
  if (!find_node_name) {
    MONITOR_WARN("EventCallBack can't find publisher name,id:" << gid << 
    " time: " << receipt_time / 1e9 << " event: " << *event);

    EventManager::EscalateEvents("", event, gid);
  }
}


void EventManager::EscalateEvents(const std::string& name, const agv_msgs::msg::AGVEvent::SharedPtr& event, const std::string& gid)
{
  ASSERT(!name.empty() || !gid.empty());
  std::lock_guard<std::mutex> lock(eventMutex);
  
  // MONITOR_DEBUG("New Event:" << *event);
  if (event->type > agv_msgs::msg::AGVEvent::ALARM) {
    MONITOR_ERROR("Wrong event type , discard,  Node: " << name << " type: " << event->type);
    return;
  }

  if (event->action > agv_msgs::msg::AGVEvent::ESTOP) {
    MONITOR_ERROR("Wrong event action, discard, Node: " << name << " action: " << event->action);
    return;
  }

  if (event->ecode > agv_msgs::msg::AGVEvent::MAX_EVENT_CODE) {
    MONITOR_ERROR("Wrong event ecode, discard, Node: " << name << " ecode: " << event->ecode);
    return;
  }

  auto EventManager = EventManager::Instance();
  auto* eventmap = EventManager->GetEventMap();

  double seconds = Monitor::GetTimes().seconds();
  double delta = seconds - event->eventtime;
  
  std::string operation_key;
  bool need_rename = false;

  if (!gid.empty() && !name.empty()) {
    auto gid_it = eventmap->find(gid);
    if (gid_it != eventmap->end()) {
      // find gid and rename eventmap keyname
      AgvEventVector existing_events = std::move(gid_it->second);
      eventmap->erase(gid_it);
      eventmap->emplace(name, std::move(existing_events));
      operation_key = name;
    } else {
      operation_key = name;
    }

  } else if (gid.empty() && !name.empty()) {
      operation_key = name;
  } else if (!gid.empty() && name.empty()) {
      operation_key = gid;
  } else {
      MONITOR_ERROR("Invalid parameters, both name and gid are empty");
      return;
  }
  auto it = eventmap->find(operation_key);
  if (it != eventmap->end()) {
      int event_id = event->ecode;
      AgvEventVector& events = it->second;
      
      auto event_it = std::find_if(events.begin(), events.end(),
          [event_id](const AgvEvent& e) { return e.second.ecode == event_id; });

      if (event_it != events.end()) {
          event_it->first = Monitor::GetTimes();
          event_it->second = *event;  // 使用直接赋值简化代码
      } else {
          events.emplace_back(Monitor::GetTimes(), *event);
      }
  } else {
      AgvEventVector new_events;
      new_events.emplace_back(Monitor::GetTimes(), *event);
      eventmap->emplace(operation_key, std::move(new_events));
      MONITOR_INFO("New Event id:" << operation_key << ", event: " << *event);
  }
}

}  // namespace monitor